#pragma config(Sensor, dgtl1,  Rencoder,       sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  Lencoder,       sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  us,             sensorSONAR_cm)
#pragma config(Sensor, dgtl11, Rtouch,         sensorTouch)
#pragma config(Sensor, dgtl12, Ltouch,         sensorTouch)
#pragma config(Motor,  port2,           Rmotor,        tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port3,           Lmotor,        tmotorVex269, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


int tSpeed=100;
int turnSpeed=50;
int bSpeed=80;

void forward(int turns)
{
  int revs= turns * 360;
  SensorValue[Rencoder]=0;
  SensorValue[Lencoder]=0;
  while(SensorValue[Lencoder]<revs)
  {
    if(-SensorValue[Rencoder]>SensorValue[Lencoder])
    {
      motor[Rmotor]=bSpeed;
      motor[Lmotor]=tSpeed;
    }
    else if(-SensorValue[Rencoder]<SensorValue[Lencoder])
    {
      motor[Rmotor]=tSpeed;
      motor[Lmotor]=bSpeed;
    }
    else if(-SensorValue[Rencoder]==SensorValue[Lencoder])
    {
      motor[Rmotor]=tSpeed;
      motor[Lmotor]=tSpeed;
    }
  }

}

void rturn(double turns)
{
  int revs= turns * 360;
  SensorValue[Rencoder]=0;
  SensorValue[Lencoder]=0;
  while(-SensorValue[Rencoder]<revs)
  {
    motor[Rmotor]=turnSpeed;
    motor[Lmotor]=-turnSpeed;
  }
}


void lturn(int turns)
{
  int revs= turns * 360;
  SensorValue[Rencoder]=0;
  SensorValue[Lencoder]=0;
  while(-SensorValue[Rencoder]<revs)
  {
    motor[Rmotor]=-turnSpeed;
    motor[Lmotor]=turnSpeed;
  }
}

void halt()
{
  motor[Rmotor]=0;
  motor[Lmotor]=0;
}

void backward(int turns)
{
  int revs= turns * 360;
  SensorValue[Rencoder]=0;
  SensorValue[Lencoder]=0;
  while(-SensorValue[Lencoder]<revs)
  {
    if(-SensorValue[Rencoder]>SensorValue[Lencoder])
    {
      motor[Rmotor]=-bSpeed;
      motor[Lmotor]=-tSpeed;
    }
    else if(-SensorValue[Rencoder]<SensorValue[Lencoder])
    {
      motor[Rmotor]=-tSpeed;
      motor[Lmotor]=-bSpeed;
    }
    else if(-SensorValue[Rencoder]==SensorValue[Lencoder])
    {
      motor[Rmotor]=-tSpeed;
      motor[Lmotor]=-tSpeed;
    }
  }
}


task main
{
  while(true)
  {
    forward(2);

    if(SensorValue[Rtouch]==1 || SensorValue[Ltouch]==1)
    {
      backward(500);
      lturn(1.1);
    }
    else if(SensorValue[Rtouch]==1 && SensorValue[Ltouch]==1)
    {
      backward(500);
      rturn(1.1);
    }
  }
}
